from fractions import Fraction
from math import sin
from math import cos

#fine the next pos of ' '
#return the end pos in the str of a point
def findNextPointLoc(line,i):
    firstSapce=i+line[i:].index(' ')
    return line[firstSapce+1:].index(' ')+firstSapce+1

def changeConf(oldPath,newPath,robot=False):

    oldFd=open(oldPath,"r")
    newFd=open(newPath,"w")
    
    #the file start with an empty line
    if robot:
        line = oldFd.readline()#read empty line
    line = oldFd.readline().strip() # number of vertices

    #write the number of polygon ~ obsteculs
    if(not robot):
        newFd.write(line+'\n')
        
    #write all of the polygon vertices
    for line in oldFd:
        line=line.strip()
        if(line==''):
            continue
        line+=' '
        nPoints=int(line[0])
        newFd.write(line[0]+'\n')
        startP=2
        #write the points of the polygon
        for i in range(0,nPoints):
            endP=findNextPointLoc(line,startP)
            newFd.write(line[startP:endP]+'\n')
            startP=endP+1
            
    oldFd.close()
    newFd.close()
    return True

def queryTarSrc(oldPath,targetPath,sourcePath):
    from math import sin
    from math import cos
    from fractions import Fraction
    
    oldFd=open(oldPath,"r")
    
    emptyLine(oldFd,4)#read empty line
    line = oldFd.readline()
    oldFd.close()
    
    source=findNextPointLoc(line,line.index(' ')+1)
    target=line[source+1:len(line)-1]
    source=line[:source]

    writeST(sourcePath,source)
    writeST(targetPath,target)

    return True

def realRat(num):
    numQ = Fraction(num)
    return str(numQ.numerator)+'/'+str(numQ.denominator)

def writeST(path , cord):
    cordX=float(cord[:cord.index(' ')])
    cordY=float(cord[cord.index(' '):cord.rfind(' ')])
    cordAng=float(cord[cord.rfind(' ')+1:])

    fd=open(path,"w")
    
    fd.write(realRat(cordX)+' ')
    fd.write(realRat(cordY)+'\n')
    fd.write(realRat(sin(cordAng))+'\n')
    fd.write(realRat(cos(cordAng)))

    fd.close()

def emptyLine(fd,n):
    for i in range(0,n):
        fd.readline()

iFd = "C:\\WH\RRT_rotating_poly\\MRMPApplication\\"
oFd = "C:\\WH\\try\\try\\"
robot = "robot_s.txt"
nr = "r.txt"
w = "workspace_A.txt"
nw = "w.txt"
q = "query.txt"
s = "s.txt"
t = "t.txt"
changeConf(iFd+robot,oFd+nr,True)
changeConf(iFd+w,oFd+nw)
queryTarSrc(iFd+q,oFd+t,oFd+s)
